#include "Helpers.h"

#include <cmath>
#include <vector>
#include <algorithm>
#include <limits>
#define EPSILON 0.00000000001

using namespace std;

core::vector3df getYXangle(core::vector3df r)
{
   vector3df angle;
   
   angle.Y = atan2 (r.X, r.Z);
   angle.Y *= (180 / PI);
   
   if(angle.Y < 0) angle.Y += 360;
   if(angle.Y >= 360) angle.Y -= 360;
   
   float z1;
   z1 = sqrt( r.X * r.X   +   r.Z * r.Z );
   
   angle.X = atan2 (z1, r.Y);
   angle.X *= (180 / PI);
   angle.X -= 90;
   
   if(angle.X < 0) angle.X += 360;
   if(angle.X >= 360) angle.X -= 360;

   return angle;
}

core::vector3df getRelativePosition(core::vector3df origin, core::vector3df rotation, core::vector3df target)
{
    core::vector3df relative = target - origin; //relative position of target
   
    core::matrix4 m; //rotation matrix
   
    //rotate target position arount origin on Z axis
    m.setRotationDegrees( core::vector3df(0,0,-rotation.Z) );
    m.transformVect(relative);
   
    //rotate target position arount origin on Y axis
    m.setRotationDegrees( core::vector3df(0,-rotation.Y,0) );
    m.transformVect(relative);
   
    //rotate target position arount origin on X axis
    m.setRotationDegrees( core::vector3df(-rotation.X,0,0) );
    m.transformVect(relative);
   
    //return relative position of target to origin and rotation
    return relative;   
}

f32 largest_root_of_quadratic_equation(f32 A, f32 B, f32 C){
  return (B+std::sqrt(B*B-4*A*C))/(2*A);
}

char	*strrtrim(char *str, const char *trim)
{
	char	*end;
	
	if(!str)
		return NULL;

	if(!trim)
		trim = " \t\n\r";
		
	end = str + strlen(str);

	while(end-- > str)
	{
		if(!strchr(trim, *end))
			return str;
		*end = 0;
	}
	return str;
}

char	*strltrim(char *str, const char *trim)
{
	if(!str)
		return NULL;
	
	if(!trim)
		trim = " \t\r\n";
	
	while(*str)
	{
		if(!strchr(trim, *str))
			return str;
		++str;
	}
	return str;
}

char	*strtrim(char *str, const char *trim)
{
	return strltrim(strrtrim(str, trim), trim);
}



vector3df getStringVector(char *str) {
	f32 val[3];
	char *pch, *context, *working;
	int x;

	if(str == NULL) 
		return vector3df(0,0,0);
	if(strlen(str) < 5) 
		return vector3df(0,0,0);

	
	if(!(working = _strdup(str)))
		return vector3df(0,0,0);
	
	x = 0;
	pch = strtok_s(working, ",", &context);

	while(pch) {
		if(x > 2) break;

		val[x++] = (f32)atof(strtrim(pch, " "));
		pch = strtok_s(NULL, ",", &context);
	}
	
	free(working);

	return vector3df(val[0], val[1], val[2]);
}



static __inline bool isnan(double value)
{
     return value != value;
}

static __inline bool isinf(double value)
{
     return value == std::numeric_limits<double>::infinity();
}

vector<double> rootsOf(double A, double B, double C, double D, double E)
{
     vector<double> roots = vector<double>();
     if (A == 0.0)
     {
          if (B == 0.0)
          {
               if (C == 0.0)
               {
                    if (D == 0.0)
                    {
                         if (E == 0.0)
                              roots.push_back(0.0);
                    }
                    else
                         roots.push_back(-D / E);
               }
               else
               {
                    roots.push_back((-D + sqrt(D * D - 4 * C * E)) / (2.0 * C));
                    roots.push_back((-D - sqrt(D * D - 4 * C * E)) / (2.0 * C));
               }
          }
          else
          {
               C /= B;
               D /= B;
               E /= B;
               double F = (3.0 * D - C * C) / 3.0;
               double G = (2.0 * C * C * C - 9.0 * C * D + 27.0 * E) / 27.0;
               double H = (G * G) / 4.0 + (F * F * F) / 27.0;
               if (H > 0)
               {
                    double intermediate = -G / 2.0 + sqrt(H);
                    double m = intermediate < 0.0 ? -pow(-intermediate, 1.0 / 3.0) : pow(intermediate, 1.0 / 3.0);
                    intermediate -= 2.0 * sqrt(H);
                    double n = intermediate < 0.0 ? -pow(-intermediate, 1.0 / 3.0) : pow(intermediate, 1.0 / 3.0);
                    roots.push_back(m + n - C / 3.0);
               }
               else
               {
                    double intermediate = sqrt(G * G / 4.0 - H);
                    double rc = intermediate < 0.0 ? -pow(-intermediate, 1.0 / 3.0) : pow(intermediate, 1.0 / 3.0);
                    double theta = acos(-G / (2.0 * intermediate)) / 3.0;
                    roots.push_back(2.0 * rc * cos(theta) - C / 3.0);
                    roots.push_back(-rc * (cos(theta) + sqrt(3.0) * sin(theta)) - C / 3.0);
                    roots.push_back(-rc * (cos(theta) - sqrt(3.0) * sin(theta)) - C / 3.0);
               }
               if (F + G + H == 0.0)
               {
                    double intermediate = E < 0.0 ? pow(-E, 1.0 / 3.0) : -pow(E, 1.0 / 3.0);
                    roots.clear();
                    roots.push_back(intermediate);
                    roots.push_back(intermediate);
                    roots.push_back(intermediate);
               }
          }
     }
     else
     {
          B /= A;
          C /= A;
          D /= A;
          E /= A;
          double F = C - (3.0 * B * B) / 8.0;
          double G = D + B * B * B / 8.0 - (B * C) / 2.0;
          double H = E - 3.0 * B * B * B * B / 256.0 + B * B * C / 16.0 - B * D / 4.0;
          double b = F / 2.0;
          double c = (F * F - 4.0 * H) / 16.0;
          double d = (G * G) / -64.0;
          double f = (3.0 * c - b * b) / 3.0;
          double g = (2.0 * b * b * b - 9.0 * b * c + 27.0 * d) / 27.0;
          double h = (g * g) / 4.0 + (f * f * f) / 27.0;
          double y1;
          double y2r;
          double y2i;
          double y3r;
          double y3i;
          if (h > 0.0)
          {
               double intermediate = -g / 2.0 + sqrt(h);
               double m = intermediate < 0.0 ? -pow(-intermediate, 1.0 / 3.0) : pow(intermediate, 1.0 / 3.0);
               intermediate -= 2.0 * sqrt(h);
               double n = intermediate < 0.0 ? -pow(-intermediate, 1.0 / 3.0) : pow(intermediate, 1.0 / 3.0);
               y1 = m + n - b / 3.0;
               y2r = (m + n) / -2.0 - b / 3.0;
               y2i = ((m - n) / 2.0) * sqrt(3.0);
               y3r = (m + n) / -2.0 - b / 3.0;
               y3i = ((m - n) / 2.0) * sqrt(3.0);
          }
          else
          {
               double intermediate = sqrt((g * g / 4.0 - h));
               double rc = intermediate < 0.0 ? -pow(-intermediate, 1.0 / 3.0) : pow(intermediate, 1.0 / 3.0);
               double theta = acos((-g / (2.0 * intermediate))) / 3.0;
               y1 = 2.0 * rc * cos(theta) - b / 3.0;
               y2r = -rc * (cos(theta) + sqrt(3.0) * sin(theta)) - b / 3.0;
               y2i = 0.0;
               y3r = -rc * (cos(theta) - sqrt(3.0) * sin(theta)) - b / 3.0;
               y3i = 0.0;
          }
          if (f + g + h == 0.0)
          {
               double intermediate = d < 0.0 ? pow(-d, 1.0 / 3.0) : -pow(d, 1.0 / 3.0);
               y1 = intermediate;
               y2r = intermediate;
               y2i = 0.0;
               y3r = intermediate;
               y3i = 0.0;
          }
          double p;
          double q;
          if (h <= 0.0)
          {
               int zeroCheck = 0;
               vector<double> cubicRoots(3);
               cubicRoots[0] = y1;
               cubicRoots[1] = y2r;
               cubicRoots[2] = y3r;
               sort(cubicRoots.begin(), cubicRoots.begin() + 2);

               p = sqrt(cubicRoots[1]);
               q = sqrt(cubicRoots[2]);

               if (abs(y1) < EPSILON)
               {
                    p = sqrt(y2r);
                    q = sqrt(y3r);
                    zeroCheck = 1;
               }
               if (abs(y2r) < EPSILON)
               {
                    p = sqrt(y1);
                    q = sqrt(y3r);
                    zeroCheck += 2;
               }
               if (abs(y3r) < EPSILON)
               {
                    p = sqrt(y1);
                    q = sqrt(y2r);
                    zeroCheck += 4;
               }
               switch (zeroCheck)
               {
                    case (3):
                         p = sqrt(y3r);
                         break;
                    case (5):
                         p = sqrt(y2r);
                         break;
                    case (6):
                         p = sqrt(y1);
                         break;
               }
               if (y1 - EPSILON < 0.0 || y2r - EPSILON < 0.0 || y3r - EPSILON < 0.0)
               {
                    if (E == 0.0)
                         roots.push_back(0.0);
               }
               else
               {
                    double r;
                    if (zeroCheck < 5)
                         r = G / (-8.0 * p * q);
                    else
                         r = 0.0;
                    double s = B / 4.0;
                    roots.push_back(p + q + r - s);
                    roots.push_back(p - q - r - s);
                    roots.push_back(-p + q - r - s);
                    roots.push_back(-p - q + r - s);
               }
          }
          else
          {
               double r2mod = sqrt(y2r * y2r + y2i * y2i);
               double y2mod = sqrt((r2mod - y2r) / 2.0);
               double x2mod = y2i / (2.0 * y2mod);
               p = x2mod + y2mod;
               double r3mod = sqrt(y3r * y3r + y3i * y3i);
               double y3mod = sqrt((r3mod - y3r) / 2.0);
               double x3mod = y3i / (2.0 * y3mod);
               q = x3mod + y3mod;
               double r = G / (-8.0 * (x2mod * x3mod + y2mod * y3mod));
               double s = B / 4.0;
               roots.push_back(x2mod + x3mod + r - s);
               roots.push_back(-x2mod - x3mod + r - s);
          }
     }
     for (int i = 0; i != roots.size(); ++i)
          if (isinf(roots[i]) || isnan(roots[i]))
               roots.erase(roots.begin() + i--);
     sort(roots.begin(), roots.end());
     return roots;
}

bool does_not_contain(vector<double> vector, double value)
{
     for(int i = 0; i != vector.size(); ++i)
          if(vector[i] == value)
               return false;
     return true;
}

void keysort(vector<double>& keys, vector<double>& items)
{
     for(int i = 0; i != keys.size(); ++i)
          for(int j = keys.size() - 1; j != i; --j)
               if(keys[j - 1] > keys[j])
               {
                    double temp = keys[j];
                    keys[j] = keys[j - 1];
                    keys[j - 1] = temp;

                    temp = items[j];
                    items[j] = items[j - 1];
                    items[j - 1] = temp;
               }
}
